


// opencv
// #include <opencv2/core/core.hpp>
#include "cv_mat.hpp"
#include "dsp_intrinsics.hpp"
#include "FFTTools.hpp"
//NOTE: FFTW support is still shaky, disabled for now.
/*#ifdef USE_FFTW
#include <fftw3.h>
#endif*/
#include "q6cache.h"

#if defined(V5_4)
#undef V5_4
#endif
#include "hvx_vector_register_defines.h"
extern "C" {
    extern void transpose_vshuff_int64(const int64_t* src, int64_t* dst, int matrix_order, int stride);
    extern void transpose_vshuff_int32(const int32_t* src, int32_t* dst, int matrix_order, int stride);
}

// namespace FFTTools
// {

wfq15_t FFT::scale() {
    return wfq15_from_int(1<<Q6_R_ct0_R(_np));
} 

// in - Q15, out: Q<log2(n)>.<15-log2(n)
// note: out is final result by multipling a scale
// note: _np=128, out:Q7.8
// note: _np=64, out:Q6.9
void FFT::fftd_chfq(const hfq15_t *in, chfq_t *out)
{
    int logN = Q6_R_ct0_R(_np);
    hfq15_t scale = hfq15_from_float(qhmath_div_f(1.f, 1<<logN));
    // 0d fft
    for (int i = 0; i < _np; i++) {
        // in - Q15, out: Q<log2(n)>.<15-log2(n)>
        qhdsp_ach_r1dfft_ah(in+i*_np, _np, _twiddles1, _twiddles2, _tmp1+i*_np);
    }
    //transpose
#if 1
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            int32_t re = Q6_R_sxth_R(_tmp1[i*_np+j]) << logN;
            int32_t im = Q6_R_asrh_R(_tmp1[i*_np+j]) << logN;
            re = wfq15_mul(re, scale);
            im = wfq15_mul(im, scale);
            _tmp2[j*_np+i] = Q6_R_combine_RlRl(im, re);
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif
    // 0d fft
    for (int i = 0; i < _np; i++) {
        qhdsp_c1dfft_ach(_tmp2+i*_np, _np, _twiddles, _tmp1+i*_np);
    }
    // transpose
#if 1
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            out[j*_np+i] = _tmp1[i*_np+j];
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp1, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif
}

// in - Q<log2(n)>.<15-log2(n), out: Q15
// note: _np=128, in:Q7.8
void FFT::rifftd_chfq(const chfq_t *in, hfq15_t *out)
{
    int logN = Q6_R_ct0_R(_np);
    // 1d fft
    for (int i = 0; i < _np; i++) {
        qhdsp_c1difft_ach(_tmp2+i*_np, _np, _twiddles, _tmp1+i*_np);
    }
    wfq15_t scale = wfq15_from_int(1<<logN);
    //transpose
#if 1
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            int32_t re = Q6_R_sxth_R(_tmp1[i*_np+j]);
            int32_t im = Q6_R_asrh_R(_tmp1[i*_np+j]);
            re = wfq15_mul(re, scale) >> logN;
            im = wfq15_mul(im, scale) >> logN;
            _tmp2[j*_np+i] = Q6_R_combine_RlRl(im, re);
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp1, (int32_t*)_tmp2, _np, _np*sizeof(int32_t));
#endif
    // 1d fft
    for (int i = 0; i < _np; i++) {
        qhdsp_ah_r1difft_ach(_tmp2+i*_np, _np, _twiddles1, _twiddles2, (int16_t*)(_tmp1+i*_np));
    }
    // transpose
#if 1
    for (int i = 0; i < _np; i++) {
        int16_t *p = (int16_t *)(_tmp1+i*_np);
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            out[j*_np+i] = p[j];
        }
    }
#else
    transpose_vshuff_int16((int32_t*)_tmp1, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif   
}
// in - Q15, out: Q<log2(n)>.<15-log2(n)
// note: out is final result by multipling a scale
// note: _np=128, out:Q7.8
void FFT::fftd(const hfq15_t *in, cwfq16_t *out)
{
    int logN = Q6_R_ct0_R(_np);
    hfq15_t scale = hfq15_from_float(qhmath_div_f(1.f, 1<<logN));
    // 0d fft
    for (int i = 0; i < _np; i++) {
        // in - Q15, out: Q<log2(n)>.<15-log2(n)>
        qhdsp_ach_r1dfft_ah(in+i*_np, _np, _twiddles1, _twiddles2, _tmp1+i*_np);
    }
    //transpose
#if 1
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            int32_t re = Q6_R_sxth_R(_tmp1[i*_np+j]) << logN;
            int32_t im = Q6_R_asrh_R(_tmp1[i*_np+j]) << logN;
            re = wfq15_mul(re, scale);
            im = wfq15_mul(im, scale);
            _tmp2[j*_np+i] = Q6_R_combine_RlRl(im, re);
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif
    // 0d fft
    for (int i = 0; i < _np; i++) {
        qhdsp_c1dfft_ach(_tmp2+i*_np, _np, _twiddles, _tmp1+i*_np);
    }
    // transpose
#if 1
    wfq15_t scale2 = wfq15_from_int(1<<logN);
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            // dst(i, j) = tmp(j, i);
            int32_t re = Q6_R_sxth_R(_tmp1[i*_np+j]) << logN;
            int32_t im = Q6_R_asrh_R(_tmp1[i*_np+j]) << logN;
            re = wfq15_mul(re, scale2) << 1; // Q15-->Q16
            im = wfq15_mul(im, scale2) << 1;
            out[j*_np+i] = Q6_P_combine_RR(im, re);
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp1, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif
}

// in - Q<log2(n)>.<15-log2(n), out: Q15
// note: _np=128, in:Q7.8
void FFT::rifftd(const cwfq16_t *in, hfq15_t *out)
{
    int logN = Q6_R_ct0_R(_np);
    // int32_t scale1 = wfq15_from_float(qhmath_div_f(1.f, 1<<logN));
    for (int i = 0; i < _np*_np; i++) {
        // int32_t re = cwfq16_re(in[i])>>1; //Q15
        // int32_t im = cwfq16_im(in[i])>>1;
        // re = wfq15_mul(re, scale1)>>8;
        // im = wfq15_mul(im, scale1)>>8;
        int32_t re = Q6_R_sath_R(cwfq16_re(in[i]) >> 8); // Q16-->Q8
        int32_t im = Q6_R_sath_R(cwfq16_im(in[i]) >> 8);
        _tmp2[i] = Q6_R_combine_RlRl(im, re);
    }
    // 1d fft
    for (int i = 0; i < _np; i++) {
        // in: Q<log2(N)>.<15-log2(N)>, out: Q15
        qhdsp_c1difft_ach(_tmp2+i*_np, _np, _twiddles, _tmp1+i*_np);
    }
    wfq15_t scale = wfq15_from_int(1<<logN); //Q15
    //transpose
#if 1
    for (int i = 0; i < _np; i++) {
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            int32_t re = Q6_R_sxth_R(_tmp1[i*_np+j]);
            int32_t im = Q6_R_asrh_R(_tmp1[i*_np+j]);
            re = wfq15_mul(re, scale) >> logN;
            im = wfq15_mul(im, scale) >> logN;
            _tmp2[j*_np+i] = Q6_R_combine_RlRl(im, re);
        }
    }
#else
    transpose_vshuff_int32((int32_t*)_tmp1, (int32_t*)_tmp2, _np, _np*sizeof(int32_t));
#endif
    // 1d fft
    for (int i = 0; i < _np; i++) {
        // in: Q<log2(N)>.<15-log2(N)>, out: Q15
        qhdsp_ah_r1difft_ach(_tmp2+i*_np, _np, _twiddles1, _twiddles2, (int16_t*)(_tmp1+i*_np));
    }
    // transpose
#if 1
    for (int i = 0; i < _np; i++) {
        int16_t *p = (int16_t *)(_tmp1+i*_np);
        for (int j = 0; j < _np; j++) {
            // dst(j, i) = tmp(i, j);
            out[j*_np+i] = p[j];
        }
    }
#else
    transpose_vshuff_int16((int32_t*)_tmp1, (int32_t*)out, _np, _np*sizeof(int32_t));
#endif   
}

void rearrange_128(CvMat_hfq15 &img)
{
    int cx = img.cols / 2;
    int cy = img.rows / 2;

    HVX_VectorPair *p_pv1 = (HVX_VectorPair*)img.data;
    HVX_VectorPair *p_pv2 = (HVX_VectorPair*)(img.data+cy*img.cols);
    HVX_Vector *p_tl = (HVX_Vector*)img.data;
    HVX_Vector *p_tr = (HVX_Vector*)(img.data + cx);
    HVX_Vector *p_bl = (HVX_Vector*)(img.data + cy*img.cols);
    HVX_Vector *p_br = (HVX_Vector*)(img.data + cy*img.cols + cx);
    long long l2fetch = CreateL2pfParam(img.colStride(), img.cols, 1, 0);
    hfq15_t *pSrc1 = img.data;
    hfq15_t *pSrc2 = img.data+cy*img.cols;
    L2fetch( (unsigned int)(pSrc1), l2fetch);
    L2fetch( (unsigned int)(pSrc2), l2fetch);
    for (int i = 0; i < cy; i++) {
        if (i < cy-1) {
            L2fetch( (unsigned int)(pSrc1+img.cols), l2fetch);
            L2fetch( (unsigned int)(pSrc2+img.cols), l2fetch);
        }
        HVX_VP V1__0, V3__2;
        V1_0 = *p_pv1;
        V3_2 = *p_pv2;
        *p_br = V0;
        *p_bl = V1;
        *p_tl = V3;
        *p_tr = V2;
        p_pv1++;
        p_pv2++;
        p_tl += 2;
        p_tr += 2;
        p_bl += 2;
        p_br += 2;
        pSrc1 += img.cols;
        pSrc2 += img.cols;
    }
}

void rearrange_64(CvMat_hfq15 &img)
{
    int cy = img.rows / 2;

    HVX_Vector *p_v1 = (HVX_Vector*)img.data;
    HVX_Vector *p_v2 = (HVX_Vector*)(img.data+cy*img.cols);
    long long l2fetch = CreateL2pfParam(img.colStride(), img.cols, 1, 0);
    hfq15_t *pSrc1 = img.data;
    hfq15_t *pSrc2 = img.data+cy*img.cols;
    L2fetch( (unsigned int)(pSrc1), l2fetch);
    L2fetch( (unsigned int)(pSrc2), l2fetch);
    for (int i = 0; i < cy; i++) {
        if (i < cy-1) {
            L2fetch( (unsigned int)(pSrc1+img.cols), l2fetch);
            L2fetch( (unsigned int)(pSrc2+img.cols), l2fetch);
        }
        HVX_VP V1__0;
        V0 = *p_v1;
        V1 = *p_v2;
        V1_0 = Q6_W_vshuff_VVR(V1, V0, -64);
        V1_0 = Q6_W_vshuff_VVR(V0, V1, -64);
        *p_v1++ = V1;
        *p_v2++ = V0;
        pSrc1 += img.cols;
        pSrc2 += img.cols;
    }
}

void rearrange_f(CvMatF32 &img)
{
    CvMatF32 tmp(img.rows, img.cols);
    int cx = img.cols / 2;
    int cy = img.rows / 2;
#if 0
    for (int i = 0; i < cy; i++) {
        for (int j = 0; j < cx; j++) {
            // top-left
            tmp(i, j) = img(cy+i, cx+j);
            // bottom-right
            tmp(cy+i, cx+j) = img(i, j);
            // top-right
            tmp(i, cx+j) = img(cy+i, j);
            // bottom-left
            tmp(cy+i, j) = img(i, cx+j);
        }
    }
#else
    for (int i = 0; i < cy; i++) {
        for (int j = 0; j < cx; j++) {
            // top-left
            tmp(i, j) = img(cy+i, cx+j);
        }
    }
    for (int i = 0; i < cy; i++) {
        for (int j = 0; j < cx; j++) {
            // bottom-right
            tmp(cy+i, cx+j) = img(i, j);
        }
    }
    for (int i = 0; i < cy; i++) {
        for (int j = 0; j < cx; j++) {
            // top-right
            tmp(i, cx+j) = img(cy+i, j);
        }
    }
    for (int i = 0; i < cy; i++) {
        for (int j = 0; j < cx; j++) {
            // bottom-left
            tmp(cy+i, j) = img(i, cx+j);
        }
    }

#endif
    img = tmp;
}

void FFT_F32::fftd(float *in, float complex *out) {
    int32_t np = _np;
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_acf_r1dfft_af(in+i*np, np, _twiddle1, _twiddle2, _tmp+i*np);
    }
    //transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1dfft_acf(out+i*np, np, _c_twiddle, _tmp+i*np);
    }
    // transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
}
// twiddles: qhdsp_fft_gen_twiddles_real_acf()
void FFT_F32::ifftd(float complex *in, float complex *out) {
    int32_t np = _np;
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_af_r1difft_acf(in, np, _twiddle1, _twiddle2, _tmp);
    }
    //transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1difft_acf(out, np, _c_twiddle, _tmp);
    }
    // transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
}

// twiddles: qhdsp_fft_gen_twiddles_complex_acf();
void FFT_F32::cfftd(float complex *in, float complex *out) {
    int32_t np = _np;
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1dfft_acf(in+i*np, np, _c_twiddle, _tmp+i*np);
    }
    //transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1dfft_acf(out+i*np, np, _c_twiddle, _tmp+i*np);
    }
    // transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
}

void FFT_F32::cifftd(float complex *in, float complex *out) {
    int32_t np = _np;
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1difft_acf(in+i*np, np, _c_twiddle, _tmp+i*np);
    }
    //transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1difft_acf(out+i*np, np, _c_twiddle, _tmp+i*np);
    }
    // transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)out, np, np*sizeof(int64_t));
}

void FFT_F32::rifftd(float complex *in, float *out) {
    int32_t np = _np;
    // 1d fft
    for (int i = 0; i < np; i++) {
        qhdsp_c1difft_acf(in+i*np, np, _c_twiddle, _tmp+i*np);
    }
    //transpose
    transpose_vshuff_int64((int64_t*)_tmp, (int64_t*)_tmp2, np, np*sizeof(int64_t));
    // 1d fft
    float *ptr = (float*)_tmp;
    for (int i = 0; i < np; i++) {
        qhdsp_af_r1difft_acf(_tmp2+i*np, np, _twiddle1, _twiddle2, (float complex*)(ptr+i*np));
    }
    // transpose
    transpose_vshuff_int32((int32_t*)_tmp, (int32_t*)out, np, np*sizeof(int32_t));
}
// }